﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Flight
{
    public class HelicopterAutopilot : IAutopilot
    {
        bool
            ap_alt,
            ap_speed,
            ap_heading;
        float
            target_vspeed,
            target_alt,
            target_speed,
            target_heading;

        public HelicopterAutopilot()
        {
            throw new Exception("Please don't instantiate me yet!");

            ap_alt = ap_speed = ap_heading = false;

            target_vspeed = 500f / 60;
            target_alt = 1000;
            target_speed = 0;
            target_heading = 0;
        }

        private float spow(float b, float e)
        {
            if (b >= 0) return (float)Math.Pow(b, e);
            return -(float)Math.Pow(-b, e);
        }

        public void update(float dt, Aircraft aircraft, ControlInputs inputs)
        {

        }

        public ControlInputs getControlInputs(Aircraft aircraft, ControlInputs inputs)
        {
            if (!(aircraft is Helicopter)) return inputs;
            Helicopter heli = (Helicopter)aircraft;

            if (ap_speed)
            {
                float diff = target_speed - heli.getVelocity();
                float pitch_diff = (float)(Math.Atan(-diff) / Math.PI / 2 * 15);
                inputs.pitch = MathHelper.Clamp(pitch_diff / 5, -1, 1);
            }
            if (ap_heading)
            {
                float diff = target_heading - heli.getHeading();
                Console.WriteLine("diff = " + diff);
                if (diff > 180)
                    diff -= 360;
                diff = -diff;
                Console.WriteLine("diff = " + diff);
                inputs.yaw = MathHelper.Clamp(spow(diff, 0.9f) / 100, -1, 1);
                Console.WriteLine("yaw = " + inputs.yaw + "\n");
            }
            if (ap_alt)
            {
                float diff = target_alt - heli.getLoc().Z;
                if (Math.Abs(diff) > target_vspeed * 5)
                {
                    float signed_vspeed = diff > 0 ? target_vspeed : -target_vspeed;
                    float vspeed_diff = signed_vspeed - heli.getVSpeed();
                    inputs.power = MathHelper.Clamp(vspeed_diff / 10 + 0.5f, 0, 1);
                }
                else
                {
                    inputs.power = MathHelper.Clamp((diff - heli.getVSpeed() * 5) / 1000 + 0.5f, 0, 1);
                }
            }

            return inputs;
        }
    }
}
